'''
	Implements a really simple joystick controller. Meant as an example how to
	implement a InputController
	
	@author: J.H. Donker, J.Teeuw
	@copyright: Twente University
'''

import wx
from InputController import TimedInputController
from InputInterface import InputWrapper	
#from math import sin, cos,  pi
import math

class JoystickController(TimedInputController):
	'''	
		The SimpleJoyStickController uses the TimedInputController to poll
		it's self every 30 miliseconds. This should give good feedback.
	'''
	
	# Joystick buttons
	BUTTON1 = 1
	BUTTON2 = 2
	BUTTON3 = 4
	BUTTON4 = 8
	BUTTON5 = 16
	BUTTON6 = 32
	BUTTON7 = 64
	BUTTON8 = 128
	
	DISTANCE = 1.00
	
	FLY_MODE = 1
	STEER_MODE = 2
	STEER_SENSITIVITY = 0.10 # increase for more sensitive
	
	
	# constructor
	def __init__(self, inputInterface):
		'''
			Constructor. Needs an inputInterface version 1.0.
			
			@param inputInterface: An inputinterface of version 1.0
		'''
		TimedInputController.__init__(self, inputInterface, 30)
		
		self.__js = wx.Joystick()
		
		self._calibrate() # First time calibration ...
		self._calibrated = False # ... but reset calibrated flag to false
		
		self._mode = self.FLY_MODE
		
		self._xinvert = False
		self._yinvert = False
		self._zinvert = False
		
		self._ixorient = 0.00
		self._iyorient = 0.00
		self._izorient = 0.00
		
	
	# calibrate joystick
	def _calibrate(self):
		'''
			Calibrate the joystick b y resetting the center position, offset and range
			
			@note: this is a private method and should not be called by the user
		'''
		self._xmin = self.__js.GetXMin()
		self._xmax = self.__js.GetXMax()
		self._ymin = self.__js.GetYMin()
		self._ymax = self.__js.GetYMax()
		self._zmin = self.__js.GetZMin()
		self._zmax = self.__js.GetZMax()
		
		self._xrange = self._xmax - self._xmin
		self._yrange = self._ymax - self._ymin
		self._zrange = self._zmax - self._zmin
		
		self._xcentre = self._xrange / 2
		self._ycentre = self._yrange / 2
		self._zcentre = self._zrange / 2
		
		self._xpos, self._ypos = self.__js.GetPosition()
		self._zpos = self.__js.GetZPosition()
		self._buttons = self.__js.GetButtonState()
		
		self._xoffset = self._xpos - self._xcentre
		self._yoffset = self._ypos - self._ycentre
		self._zoffset = self._zpos - self._zcentre
		
		self._calibrated = True
		
	
	def IsButtonDown(self, button):
		'''
			Check if a button on the joysick is pressed down
			
			@param button: the button to check for
			@note: this is a private method and should not be called by the user
		'''
		return self.__js.GetButtonState() & button # NOTE: returns an integer, not a boolean value!!
		
	
	def SwitchMode(self):
		'''
			Cycle through the different modes of manipulation
			
			@note: this is a private method and should not be called by the user
		'''
		if self._mode == self.FLY_MODE:
			self._mode = self.STEER_MODE
		else:
			self._mode = self.FLY_MODE
			
		
	def DoWork(self):
		'''
			The actual workhorse horse method. Polls the joystick and gives 
			position updates to the system.
		'''
		
		# update position and button state
		self._xpos, self._ypos = self.__js.GetPosition()
		self._zpos = self.__js.GetZPosition()
		self._buttons = self.__js.GetButtonState()
		
		# calibrate joystick
		if self.IsButtonDown(self.BUTTON5):
			print "calibrated"
			self._calibrate()
		elif self.IsButtonDown(self.BUTTON6):
			self.SwitchMode()
			print "swithed mode to: " + str(self._mode)
		elif self.IsButtonDown(self.BUTTON3):
			self._xinvert = not self._xinvert
			print "invert x-axis"
		elif self.IsButtonDown(self.BUTTON4):
			self._yinvert = not self._yinvert
			print "invert y-axis"
			
		# control instrument
		if self._calibrated:
		
			# set orientation according to joystick position
			xorient = (float(self._xpos) / self._xrange) * -360 + 180.0 # NOTE: inverted because of different alignment of axes (left/right)
			yorient = (float(self._ypos) / self._yrange) * 360 - 180.0 # NOTE: (up/down)
			#zorient = (float(self._zpos) / self._zrange) * 360 - 180.0
			zorient = 0
			
			if self._xinvert:
				xorient = -xorient
			
			if self._yinvert:
				yorient = -yorient
			
			if self._zinvert:
				zorient = -zorient
			
			#ixorient, iyorient, izorient = self._inputInterface.GetOrientationInstrument()
			
			if self._mode == self.STEER_MODE:
				self._ixorient = zorient # NOTE: swapped because of different alignment of axes
				self._iyorient = xorient # NOTE: swapped because of different alignment of axes
				self._izorient = yorient # NOTE: swapped because of different alignment of axes
			elif self._mode == self.FLY_MODE:
				self._ixorient = self._ixorient + (zorient) * self.STEER_SENSITIVITY # NOTE: swapped because of different alignment of axes
				self._iyorient = self._iyorient + (xorient) * self.STEER_SENSITIVITY # NOTE: swapped because of different alignment of axes
				self._izorient = self._izorient + (yorient) * self.STEER_SENSITIVITY # NOTE: swapped because of different alignment of axes
			
			self._inputInterface.SetOrientationInstrument(self._ixorient, self._iyorient, self._izorient)
			
			# set translation according to button state
			degtorad = math.pi / 180.0

			# vec * yrot * zrot
			dx = math.cos(self._iyorient * degtorad) * math.cos(self._izorient * degtorad)
			dy = math.sin(self._izorient * degtorad) * math.cos(self._iyorient * degtorad)
			dz = -math.sin(self._iyorient * degtorad)
			
			ixpos, iypos, izpos = self._inputInterface.GetPositionInstrument()
			
			if self.IsButtonDown(self.BUTTON1):
				ixpos = ixpos + dx * self.DISTANCE
				iypos = iypos + dy * self.DISTANCE
				izpos = izpos + dz * self.DISTANCE
			elif self.IsButtonDown(self.BUTTON2):
				ixpos = ixpos - dx * self.DISTANCE
				iypos = iypos - dy * self.DISTANCE
				izpos = izpos - dz * self.DISTANCE
				 
			self._inputInterface.SetPositionInstrument(ixpos, iypos, izpos)
		
		else:
			# temp
			print "Joystick is not calibrated! Move is around and press button #5" # TODO: remove!
		